/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Robot;

import lejos.robotics.TachoMotor;

/**
 *
 * @author zach
 */
public class CompassPilot extends TachoPilot{

    public CompassPilot() {
     super();
    }

    public CompassPilot(float wheelDiameter, float trackWidth, TachoMotor leftMotor, TachoMotor rightMotor) {
        super(wheelDiameter, trackWidth, leftMotor, rightMotor);
    }

    public CompassPilot(float wheelDiameter, float trackWidth, TachoMotor leftMotor, TachoMotor rightMotor, boolean reverse) {
        super(wheelDiameter, trackWidth, leftMotor, rightMotor, reverse);
    }

    public CompassPilot(float leftWheelDiameter, float rightWheelDiameter, float trackWidth, TachoMotor leftMotor, TachoMotor rightMotor, boolean reverse) {
        super(leftWheelDiameter, rightWheelDiameter, trackWidth, leftMotor, rightMotor, reverse);
    }

    @Override
    public void rotate(float angle,boolean ret) {
        float baring=Configureation.compassSensor().getDegreesCartesian();
        super.rotate(angle,ret);
        float fin=Configureation.compassSensor().getDegreesCartesian();
        if(angle<180){
        getLeft().setSpeed(450);
        getRight().setSpeed(-450);
        }else{
        getLeft().setSpeed(-450);
        getRight().setSpeed(450); 
        }
        getRight().smoothAcceleration(true);
        getLeft().smoothAcceleration(true);
        while(true){
            if(almostEqule(subWithAdjustemnt(baring,fin),subWithAdjustemnt(baring,angle))){
                break;
            }else if(subWithAdjustemnt(angle,fin)>subWithAdjustemnt(baring,fin)){
                getRight().rotate(3, ret);
                getLeft().rotate(3, ret);
            }else if(subWithAdjustemnt(angle,fin)<subWithAdjustemnt(baring,fin)){
                getRight().rotate(-3, ret);
                getLeft().rotate(-3, ret);
            }
            fin=Configureation.compassSensor().getDegreesCartesian();
        }
    }

    public void rotate(float angle){
        this.rotate(angle, false);
    }

    private boolean almostEqule(float num1,float num2){
        if(num1-num2<2f||num2-num1<2f)
            return true;
        else
            return false;
    }

    private float subWithAdjustemnt(float angle1, float angle2){
        if(angle1-angle2>0)
            return angle1-angle2;
        else
            return subWithAdjustemnt(angle1+360,angle2);
    }
}
